//
// Created by yunnan xu on 2020/12/8.
//

#include "RigidBody.h"
#include "gtx/quaternion.hpp"
#include <utility>

namespace Phy {


    RigidBody::RigidBody(float m, glm::mat3x3 _IBody, const std::shared_ptr<Convex> & _convex, std::shared_ptr<Model> model) :
        convex(_convex),
        mass(m),
        IBody(_IBody),
        model_ptr(model)
    {
        IBodyInv = glm::inverse(IBody);
        force = glm::vec3();
        torque = glm::vec3();
        v = glm::vec3(0.0f);
        w = glm::vec3(0.0f);
        R = glm::mat3x3 (1.0f);
        P = glm::vec3(0.0f);
        L = glm::vec3(0.0f);
        quaternion = glm::toQuat(R);
        is_static = false;
    }

    void RigidBody::SetV(glm::vec3 lv)
    {
        v = lv;
    }

    void RigidBody::SetW(glm::vec3 av)
    {
        w = av;
    }

    glm::vec3 RigidBody::GetV() const
    {
        return v;
    }

    glm::vec3 RigidBody::GetW() const
    {
        return w;
    }

    void RigidBody::AddImpulseAt(glm::vec3 position, glm::vec3 impulse)
    {
    }

    glm::mat3x3 RigidBody::StarMat(glm::vec3 vec)
    {
        glm::mat3x3 ret;

        ret[0][0] = 0.0f; ret[0][1] = -vec.z; ret[0][2] = vec.y;
        ret[1][0] = vec.z; ret[1][1] = 0.0f; ret[1][2] = -vec.x;
        ret[2][0] = -vec.y; ret[2][1] = vec.x; ret[2][2] = 0.0f;

        return ret;
    }

    void RigidBody::SetMass(float m)
    {
        mass = m;
    }

    void RigidBody::SetStatic()
    {
        is_static = true;
        mass = FLOAT_MAX;
        IBodyInv = glm::mat3x3(0.0);
    }

    void RigidBody::Update(double elapsed_time)
    {
        if (is_static) return;

        // 平动
        auto dt = (float) elapsed_time;
        glm::vec3 a = force / mass;
        v += dt * a;
        center += dt * v;

        auto Rt = glm::transpose(R);
        // 转动 先计算惯性张量
        Iinv = R * IBodyInv * Rt;

        //LOGMat("Iinv", Iinv);
        //LOGVec("L before", L);

        // 转动 再计算角动量
        L += torque * dt;

        auto dR = StarMat(w) * R * dt;

        // 转动 最后计算旋转矩阵
        R += dR;

        quaternion = glm::normalize(glm::toQuat(R));
        R = glm::toMat3(quaternion);

        // 转动 再计算角速度
        w = L * Iinv;
        //LOGVec("L after", L);
        //LOGVec("w", w);
        //LOGMat("R", R);
        //LOGMat("dR", dR);

        auto trans_mat = GetTranslationMat();
        auto rotate_mat = GetRotateMat();

        //LOGMat("trans_mat", trans_mat);
        //LOGMat("rotate_mat", rotate_mat);
        //LOGMat("scale_mat", scale_mat);

        convex->UpdateWithModelMatrix( trans_mat * rotate_mat * scale_mat);
    }

    glm::mat4 RigidBody::GetTranslationMat()
    {
        glm::mat4 translation_mat = glm::mat4(1.0f);
        translation_mat[3][0] = center.x;
        translation_mat[3][1] = center.y;
        translation_mat[3][2] = center.z;
        return translation_mat;
    }

    glm::mat4 RigidBody::GetRotateMat()
    {
        return R;
    }

    void RigidBody::SetForce(glm::vec3 f)
    {
        force = f;
    }
    void RigidBody::SetTorque(glm::vec3 t)
    {
        torque = t;
    }

    void RigidBody::SetTranslationMat(glm::mat4 mat)
    {
        center.x = mat[3][0];
        center.y = mat[3][1];
        center.z = mat[3][2];
    }

    void RigidBody::SetRotateMat(glm::mat4 mat)
    {
        R[0][0] = mat[0][0]; R[0][1] = mat[0][1]; R[0][2] = mat[0][2];
        R[1][0] = mat[1][0]; R[1][1] = mat[1][1]; R[1][2] = mat[1][2];
        R[2][0] = mat[2][0]; R[2][1] = mat[2][1]; R[2][2] = mat[2][2];

        quaternion = glm::normalize(glm::toQuat(R));
    }

    glm::mat4 RigidBody::GetScaleMat() const
    {
        return scale_mat;
    }

    void RigidBody::SetScaleMat(glm::mat4 mat)
    {
        scale_mat = mat;
    }

    std::shared_ptr<Model> RigidBody::GetModel()
    {
        return model_ptr;
    }
}